{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# AHRSFactor\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/AHRSFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `AHRSFactor` class implements a factor to implement an *Attitude and Heading Reference System* (AHRS) within GTSAM. It is a binary factor, taking preintegrated measurements from a gyroscope between two discrete time steps, typically denoted as $t_i$ and $t_j$. These preintegrated measurements encapsulate the rotational motion observed by an inertial measurement unit (IMU) between these two timestamps.\n",
    "\n",
    "The `AHRSFactor` thus constrains two attitude states (represented as elements of $SO(3)$) based solely on gyroscope measurements. Accelerometer or magnetometer aiding, needed to build a complete AHRS system, must be handled separately."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Note: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install --quiet gtsam-develop plotly"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Mathematical Formulation\n",
    "\n",
    "The `AHRSFactor` relies on the use of `PreintegratedRotation`, which applies successive exponential maps to each individual gyroscope measurement $\\omega$ over the interval $[t_i, t_j]$. In this approach, every measurement contributes a small rotation given by $\\exp(\\omega_k \\Delta t)$, and the overall preintegrated rotation is obtained by composing these incremental rotations:\n",
    "$$\n",
    "\\Delta R_{ij}^{meas} = \\prod_{k} \\exp(\\omega_k \\Delta t)\n",
    "$$\n",
    "\n",
    "Given two estimated rotations $R_i$ at time $t_i$ and $R_j$ at time $t_j$, the factor enforces:\n",
    "$$\n",
    "R_j \\approx R_i \\cdot \\Delta R_{ij}^{meas}\n",
    "$$\n",
    "\n",
    "The error term optimized by the factor graph is the rotational discrepancy captured by the logarithmic map:\n",
    "$$\n",
    "e = \\text{Log}\\left((\\Delta R_{ij}^{meas})^T \\cdot R_i^T R_j\\right)\n",
    "$$\n",
    "\n",
    "Note: the reality is a bit more complicated, because the code also takes into account the effects of bias, and if desired, coriolis forces."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality\n",
    "\n",
    "### Constructor\n",
    "\n",
    "The constructor initializes the factor using a preintegrated AHRS measurement object, relating orientation states at discrete time instances $t_i$ and $t_j$.\n",
    "\n",
    "### Core Methods\n",
    "\n",
    "- `evaluateError`: calculates the error between two estimated orientations at times $t_i$ and $t_j$, relative to the preintegrated gyroscopic measurements. The error is computed as:\n",
    "\n",
    "  $$\n",
    "  \\text{error} = \\text{Log}\\left((\\Delta R_{ij}^{meas})^T \\cdot R_i^T R_j\\right)\n",
    "  $$\n",
    "\n",
    "  Here:\n",
    "\n",
    "  - $R_i, R_j$ are the estimated rotation matrices at times $t_i$ and $t_j$.\n",
    "  - $\\Delta R_{ij}^{meas}$ is the rotation matrix obtained by integrating gyroscope measurements from $t_i$ to $t_j$.\n",
    "  - $\\text{Log}(\\cdot)$ is the logarithmic map from $SO(3)$ to $\\mathbb{R}^3$.\n",
    "\n",
    "  The resulting 3-dimensional error vector represents the rotational discrepancy.\n",
    "\n",
    "- `print`: outputs a readable representation of the internal state of the factor, including the associated time steps and preintegrated measurements, aiding debugging and verification.\n",
    "\n",
    "- `equals` determines if another `AHRSFactor` instance is identical, useful in testing scenarios or when verifying the correctness of factor graph constructions."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage\n",
    "\n",
    "First, create a PreintegratedAhrsMeasurements (PAM) object by supplying the necessary IMU parameters. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from gtsam import PreintegrationParams, PreintegratedAhrsMeasurements\n",
    "\n",
    "params = PreintegrationParams.MakeSharedU(9.81)\n",
    "params.setGyroscopeCovariance(np.deg2rad(1)*np.eye(3))\n",
    "params.setAccelerometerCovariance(0.01*np.eye(3))\n",
    "\n",
    "biasHat = np.zeros(3)  # Assuming no bias for simplicity\n",
    "pam = PreintegratedAhrsMeasurements(params, biasHat)  "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Next, integrate each gyroscope measurement along with its corresponding time interval to accumulate the preintegrated rotation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "from gtsam import Point3\n",
    "np.random.seed(42)  # For reproducibility\n",
    "for _ in range(15):  # Add 15 random measurements, biased to move around z-axis\n",
    "    omega = Point3(0,0,-0.5) + 0.1*np.random.randn(3)  # Random angular velocity vector\n",
    "    pam.integrateMeasurement(omega, deltaT=0.1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, construct the AHRSFactor using the accumulated PAM and the keys representing the rotation states at the two time instants."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "from gtsam import AHRSFactor\n",
    "bias_key = 0  # Key for the bias\n",
    "i, j = 1, 2  # Indices of the two attitude unknowns\n",
    "ahrs_factor = AHRSFactor(i, j, bias_key, pam)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "AHRSFactor(1,2,0,  preintegrated measurements:    deltaTij [1.5]\n",
      "    deltaRij.ypr = (  -0.82321 -0.0142842  0.0228577)\n",
      "biasHat [0 0 0]\n",
      " PreintMeasCov [   0.0261799 1.73472e-18 1.35525e-20\n",
      "1.73472e-18   0.0261799 9.23266e-20\n",
      "1.35525e-20 1.17738e-19   0.0261799 ]\n",
      "  noise model: diagonal sigmas [0.161802159; 0.161802159; 0.161802159];\n",
      "\n"
     ]
    }
   ],
   "source": [
    "print(ahrs_factor)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [AHRSFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.h)\n",
    "- [AHRSFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.cpp)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
